
#include <BCube>
#include "ICubeCubeAlgorithm.h"
#include "IContinuousConvexCast.h"
#include "member_BNode.h"
#include "member_BBody.h"

using namespace BWE;

ICubeCubeAlgorithm::ICubeCubeAlgorithm()
{

}
ICubeCubeAlgorithm::~ICubeCubeAlgorithm()
{

}

inline BReal Dotpq(const BReal* a, const BReal* b, int p, int q)
{
	return a[0] * b[0] + a[p] * b[q] + a[2 * p] * b[2 * q];
}
static BReal Dot11(const BReal* a, const BReal* b)
{
	return Dotpq(a, b, 1, 1);
}
static BReal Dot44(const BReal* a, const BReal* b)
{
	return Dotpq(a, b, 4, 4);
}
static BReal Dot41(const BReal* a, const BReal* b)
{
	return Dotpq(a, b, 4, 1);
}
static BReal Dot14(const BReal* a, const BReal* b)
{
	return Dotpq(a, b, 1, 4);
}
void Multiply1(BVector& A, const BReal* B, const BVector& C)
{
	A[0] = Dot41(B + 0, C);
	A[1] = Dot41(B + 1, C);
	A[2] = Dot41(B + 2, C);
}
void Multiply0(BVector& A, const BReal* B, const BVector& C)
{
	A[0] = Dot11(B + 0, C);
	A[1] = Dot11(B + 4, C);
	A[2] = Dot11(B + 8, C);
}
void LineClosestApproach(const BVector& pa, const BVector& ua, const BVector& pb, const BVector& ub, BReal* alpha, BReal* beta)
{
	BVector p;
	p[0] = pb[0] - pa[0];
	p[1] = pb[1] - pa[1];
	p[2] = pb[2] - pa[2];
	BReal uaub = Dot11(ua, ub);
	BReal q1 = Dot11(ua, p);
	BReal q2 = -Dot11(ub, p);
	BReal d = 1 - uaub * uaub;
	if (d <= BReal(0.0001f))
	{
		*alpha = 0;
		*beta = 0;
	}
	else
	{
		d = 1 / d;
		*alpha = (q1 + uaub * q2) * d;
		*beta = (uaub * q1 + q2) * d;
	}
}
int IntersectRectQuad2(BReal h[2], BReal p[8], BReal ret[16])
{
	int nq = 4, nr = 0;
	BReal buffer[16];
	BReal* q = p;
	BReal* r = ret;
	for (int dir = 0; dir <= 1; dir++)
	{
		for (int sign = -1; sign <= 1; sign += 2)
		{
			BReal* pq = q;
			BReal* pr = r;
			nr = 0;
			for (int i = nq; i > 0; i--)
			{
				if (sign * pq[dir] < h[dir])
				{
					pr[0] = pq[0];
					pr[1] = pq[1];
					pr += 2;
					nr++;
					if (nr & 8)
					{
						q = r;
						goto done;
					}
				}
				BReal* nextq = (i > 1) ? pq + 2 : q;
				if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir]))
				{
					pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) / (nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]);
					pr[dir] = sign * h[dir];
					pr += 2;
					nr++;
					if (nr & 8)
					{
						q = r;
						goto done;
					}
				}
				pq += 2;
			}
			q = r;
			r = (q == ret) ? buffer : ret;
			nq = nr;
		}
	}
done:
	if (q != ret)
		memcpy(ret, q, nr * 2 * sizeof(BReal));
	return nr;
}
void CullPoints(int n, BReal p[], int m, int i0, int iret[])
{
	int i, j;
	BReal cx, cy, q;
	if (n == 1)
	{
		cx = p[0];
		cy = p[1];
	}
	else if (n == 2)
	{
		cx = BReal(0.5) * (p[0] + p[2]);
		cy = BReal(0.5) * (p[1] + p[3]);
	}
	else
	{
		BReal a = 0;
		cx = 0;
		cy = 0;
		for (i = 0; i < (n - 1); i++)
		{
			q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
			a += q;
			cx += q * (p[i * 2] + p[i * 2 + 2]);
			cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
		}
		q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
		if (fabs(a + q) > REAL_EPSILON)
		{
			a = 1 / (3 * (a + q));
		}
		else
		{
			a = REAL_LARGE;
		}
		cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
		cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
	}

	// compute the angle of each point w.r.t. the centroid
	BReal A[8];
	for (i = 0; i < n; i++)
		A[i] = atan2(p[i * 2 + 1] - cy, p[i * 2] - cx);

	// search for points that have angles closest to A[i0] + i*(2*pi/m).
	int avail[8];
	for (i = 0; i < n; i++)
		avail[i] = 1;
	avail[i0] = 0;
	iret[0] = i0;
	iret++;
	for (j = 1; j < m; j++)
	{
		BReal a = BReal(j) * (2 * PI / m) + A[i0];
		if (a > PI)
			a -= 2 * PI;
		BReal maxdiff = 1e9, diff;

		*iret = i0;  // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0

		for (i = 0; i < n; i++)
		{
			if (avail[i])
			{
				diff = fabs(A[i] - a);
				if (diff > PI)
					diff = 2 * PI - diff;
				if (diff < maxdiff)
				{
					maxdiff = diff;
					*iret = i;
				}
			}
		}
		avail[*iret] = 0;
		iret++;
	}
}

void ICubeCubeAlgorithm::detecting(const BVector& p1, const BReal* R1, const BVector& side1, const BVector& p2, const BReal* R2, const BVector& side2)
{
	int maxc = 4;
	BVector normal;
	BReal distance;
	const BReal fudge_factor = BReal(1.05);
	BVector p;
	BVector pp;
	BVector normalC;
	const BReal* normalR = 0;
	BReal A[3], B[3];
	BReal R11, R12, R13, R21, R22, R23, R31, R32, R33;
	BReal Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33;
	BReal s, s2, len;
	int i, j, invert_normal, code;

	// get vector from centers of box 1 to box 2, relative to box 1
	p = p2 - p1;
	Multiply1(pp, R1, p);

	// get side lengths / 2
	A[0] = side1[0] * BReal(0.5);
	A[1] = side1[1] * BReal(0.5);
	A[2] = side1[2] * BReal(0.5);
	B[0] = side2[0] * BReal(0.5);
	B[1] = side2[1] * BReal(0.5);
	B[2] = side2[2] * BReal(0.5);

	// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
	R11 = Dot44(R1 + 0, R2 + 0);
	R12 = Dot44(R1 + 0, R2 + 1);
	R13 = Dot44(R1 + 0, R2 + 2);
	R21 = Dot44(R1 + 1, R2 + 0);
	R22 = Dot44(R1 + 1, R2 + 1);
	R23 = Dot44(R1 + 1, R2 + 2);
	R31 = Dot44(R1 + 2, R2 + 0);
	R32 = Dot44(R1 + 2, R2 + 1);
	R33 = Dot44(R1 + 2, R2 + 2);

	Q11 = fabs(R11);
	Q12 = fabs(R12);
	Q13 = fabs(R13);
	Q21 = fabs(R21);
	Q22 = fabs(R22);
	Q23 = fabs(R23);
	Q31 = fabs(R31);
	Q32 = fabs(R32);
	Q33 = fabs(R33);

	// for all 15 possible separating axes:
	//   * see if the axis separates the boxes. if so, return 0.
	//   * find the depth of the penetration along the separating axis (s2)
	//   * if this is the largest depth so far, record it.
	// the normal vector will be set to the separating axis with the smallest
	// depth. note: normalR is set to point to a column of R1 or R2 if that is
	// the smallest depth normal so far. otherwise normalR is 0 and normalC is
	// set to a vector relative to body 1. invert_normal is 1 if the sign of
	// the normal should be flipped.

#define TST(expr1, expr2, norm, cc)		\
	s2 = fabs(expr1) - (expr2);			\
	if (s2 > 0) return;					\
	if (s2 > s)							\
	{									\
		s = s2;							\
		normalR = norm;					\
		invert_normal = ((expr1) < 0);	\
		code = (cc);					\
	}

	s = -REAL_MAX;
	invert_normal = 0;
	code = 0;

	// separating axis = u1,u2,u3
	TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1);
	TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2);
	TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3);

	// separating axis = v1,v2,v3
	TST(Dot41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4);
	TST(Dot41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5);
	TST(Dot41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6);

	// note: cross product axes need to be scaled when s is computed.
	// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1, expr2, n1, n2, n3, cc)				\
	s2 = fabs(expr1) - (expr2);							\
	if (s2 > REAL_EPSILON)								\
		return;											\
	len = sqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3));\
	if (len > REAL_EPSILON)								\
	{													\
		s2 /= len;										\
		if (s2 * fudge_factor > s)						\
		{												\
			s = s2;										\
			normalR = 0;								\
			normalC[0] = (n1) / len;					\
			normalC[1] = (n2) / len;					\
			normalC[2] = (n3) / len;					\
			invert_normal = ((expr1) < 0);				\
			code = (cc);								\
		}												\
	}

	BReal fudge2(1.0e-5f);

	Q11 += fudge2;
	Q12 += fudge2;
	Q13 += fudge2;

	Q21 += fudge2;
	Q22 += fudge2;
	Q23 += fudge2;

	Q31 += fudge2;
	Q32 += fudge2;
	Q33 += fudge2;

	// separating axis = u1 x (v1,v2,v3)
	TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7);
	TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8);
	TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9);

	// separating axis = u2 x (v1,v2,v3)
	TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10);
	TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11);
	TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12);

	// separating axis = u3 x (v1,v2,v3)
	TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13);
	TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14);
	TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15);

#undef TST

	if (!code)
		return;

	// if we get to this point, the boxes interpenetrate. compute the normal in global coordinates.
	if (normalR)
	{
		normal[0] = normalR[0];
		normal[1] = normalR[4];
		normal[2] = normalR[8];
	}
	else
	{
		Multiply0(normal, R1, normalC);
	}
	if (invert_normal)
	{
		normal[0] = -normal[0];
		normal[1] = -normal[1];
		normal[2] = -normal[2];
	}
	distance = -s;

	// compute contact point(s)
	if (code > 6)
	{
		// an edge from box 1 touches an edge from box 2.
		// find a point pa on the intersecting edge of box 1
		BVector pointA;
		BReal sign;
		for (i = 0; i < 3; i++)
			pointA[i] = p1[i];
		for (j = 0; j < 3; j++)
		{
			sign = (Dot14(normal, R1 + j) > 0) ? BReal(1.0) : BReal(-1.0);
			for (i = 0; i < 3; i++)
				pointA[i] += sign * A[j] * R1[i * 4 + j];
		}

		// find a point pb on the intersecting edge of box 2
		BVector pointB;
		for (i = 0; i < 3; i++)
			pointB[i] = p2[i];
		for (j = 0; j < 3; j++)
		{
			sign = (Dot14(normal, R2 + j) > 0) ? BReal(-1.0) : BReal(1.0);
			for (i = 0; i < 3; i++)
				pointB[i] += sign * B[j] * R2[i * 4 + j];
		}

		BReal alpha, beta;
		BVector ua, ub;
		for (i = 0; i < 3; i++)
			ua[i] = R1[((code)-7) / 3 + i * 4];
		for (i = 0; i < 3; i++)
			ub[i] = R2[((code)-7) % 3 + i * 4];

		LineClosestApproach(pointA, ua, pointB, ub, &alpha, &beta);
		for (i = 0; i < 3; i++)
			pointA[i] += ua[i] * alpha;
		for (i = 0; i < 3; i++)
			pointB[i] += ub[i] * beta;

		this->addContactPoint(pointB, -normal, -distance);
		return;
	}

	// okay, we have a face-something intersection (because the separating
	// axis is perpendicular to a face). define face 'a' to be the reference
	// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
	// the incident face (the closest face of the other box).
	const BReal* Ra, * Rb, * pa, * pb, * Sa, * Sb;
	if (code <= 3)
	{
		Ra = R1;
		Rb = R2;
		pa = p1;
		pb = p2;
		Sa = A;
		Sb = B;
	}
	else
	{
		Ra = R2;
		Rb = R1;
		pa = p2;
		pb = p1;
		Sa = B;
		Sb = A;
	}

	// nr = normal vector of reference face dotted with axes of incident box.
	// anr = absolute values of nr.
	BVector normal2, nr, anr;
	if (code <= 3)
	{
		normal2[0] = normal[0];
		normal2[1] = normal[1];
		normal2[2] = normal[2];
	}
	else
	{
		normal2[0] = -normal[0];
		normal2[1] = -normal[1];
		normal2[2] = -normal[2];
	}
	Multiply1(nr, Rb, normal2);
	anr[0] = fabs(nr[0]);
	anr[1] = fabs(nr[1]);
	anr[2] = fabs(nr[2]);

	// find the largest compontent of anr: this corresponds to the normal
	// for the indident face. the other axis numbers of the indicent face
	// are stored in a1,a2.
	int lanr, a1, a2;
	if (anr[1] > anr[0])
	{
		if (anr[1] > anr[2])
		{
			a1 = 0;
			lanr = 1;
			a2 = 2;
		}
		else
		{
			a1 = 0;
			a2 = 1;
			lanr = 2;
		}
	}
	else
	{
		if (anr[0] > anr[2])
		{
			lanr = 0;
			a1 = 1;
			a2 = 2;
		}
		else
		{
			a1 = 0;
			a2 = 1;
			lanr = 2;
		}
	}

	// compute center point of incident face, in reference-face coordinates
	BVector center;
	if (nr[lanr] < 0)
	{
		for (i = 0; i < 3; i++)
			center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
	}
	else
	{
		for (i = 0; i < 3; i++)
			center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
	}

	// find the normal and non-normal axis numbers of the reference box
	int codeN, code1, code2;
	if (code <= 3)
		codeN = code - 1;
	else
		codeN = code - 4;
	if (codeN == 0)
	{
		code1 = 1;
		code2 = 2;
	}
	else if (codeN == 1)
	{
		code1 = 0;
		code2 = 2;
	}
	else
	{
		code1 = 0;
		code2 = 1;
	}

	BReal quad[8];
	BReal c1, c2, m11, m12, m21, m22;
	c1 = Dot14(center, Ra + code1);
	c2 = Dot14(center, Ra + code2);

	m11 = Dot44(Ra + code1, Rb + a1);
	m12 = Dot44(Ra + code1, Rb + a2);
	m21 = Dot44(Ra + code2, Rb + a1);
	m22 = Dot44(Ra + code2, Rb + a2);
	{
		BReal k1 = m11 * Sb[a1];
		BReal k2 = m21 * Sb[a1];
		BReal k3 = m12 * Sb[a2];
		BReal k4 = m22 * Sb[a2];
		quad[0] = c1 - k1 - k3;
		quad[1] = c2 - k2 - k4;
		quad[2] = c1 - k1 + k3;
		quad[3] = c2 - k2 + k4;
		quad[4] = c1 + k1 + k3;
		quad[5] = c2 + k2 + k4;
		quad[6] = c1 + k1 - k3;
		quad[7] = c2 + k2 - k4;
	}

	// find the size of the reference face
	BReal rect[2];
	rect[0] = Sa[code1];
	rect[1] = Sa[code2];

	// intersect the incident and reference faces
	BReal ret[16];
	int n = IntersectRectQuad2(rect, quad, ret);
	if (n < 1)
		return;  // this should never happen

	// convert the intersection points into reference-face coordinates,
	// and compute the contact position and depth for each point. only keep
	// those points that have a positive (penetrating) depth. delete points in
	// the 'ret' array as necessary so that 'point' and 'ret' correspond.
	BReal point[3 * 8];  // penetrating contact points
	BReal dep[8];        // depths for those points
	BReal det1 = 1 / (m11 * m22 - m12 * m21);
	m11 *= det1;
	m12 *= det1;
	m21 *= det1;
	m22 *= det1;
	int cnum = 0;  // number of penetrating contact points found
	for (j = 0; j < n; j++)
	{
		BReal k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
		BReal k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
		for (i = 0; i < 3; i++)
		{
			point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
		}
		dep[cnum] = Sa[codeN] - Dot11(normal2, point + cnum * 3);
		if (dep[cnum] >= 0)
		{
			ret[cnum * 2] = ret[j * 2];
			ret[cnum * 2 + 1] = ret[j * 2 + 1];
			cnum++;
		}
	}
	if (cnum < 1)
		return;  // this should never happen

	// we can't generate more contacts than we actually have
	if (maxc > cnum)
		maxc = cnum;
	if (maxc < 1)
		maxc = 1;

	if (cnum <= maxc)
	{
		if (code < 4)
		{
			for (j = 0; j < cnum; j++)
			{
				BVector contactPoint;
				contactPoint.x() = point[j * 3 + 0] + pa[0];
				contactPoint.y() = point[j * 3 + 1] + pa[1];
				contactPoint.z() = point[j * 3 + 2] + pa[2];
				this->addContactPoint(contactPoint, -normal, -dep[j]);
			}
		}
		else
		{
			for (j = 0; j < cnum; j++)
			{
				BVector contactPoint;
				contactPoint.x() = point[j * 3 + 0] + pa[0] - normal[0] * dep[j];
				contactPoint.y() = point[j * 3 + 1] + pa[1] - normal[1] * dep[j];
				contactPoint.z() = point[j * 3 + 2] + pa[2] - normal[2] * dep[j];
				this->addContactPoint(contactPoint, -normal, -dep[j]);
			}
		}
	}
	else
	{
		int i1 = 0;
		BReal maxdepth = dep[0];
		for (i = 1; i < cnum; i++)
		{
			if (dep[i] > maxdepth)
			{
				maxdepth = dep[i];
				i1 = i;
			}
		}

		int iret[8];
		CullPoints(cnum, ret, maxc, i1, iret);

		for (j = 0; j < maxc; j++)
		{
			BVector contactPoint;
			contactPoint.x() = point[iret[j] * 3 + 0] + pa[0];
			contactPoint.y() = point[iret[j] * 3 + 2] + pa[1];
			contactPoint.z() = point[iret[j] * 3 + 3] + pa[2];
			if (code < 4)
				this->addContactPoint(contactPoint, -normal, -dep[iret[j]]);
			else
				this->addContactPoint(contactPoint, -normal - normal * dep[iret[j]], -dep[iret[j]]);
		}
		cnum = maxc;
	}
}

void ICubeCubeAlgorithm::caculate()
{

	const BCube* cubeA = dynamic_cast<BCube*>(_shapeA);
	const BCube* cubeB = dynamic_cast<BCube*>(_shapeB);

	const BMatrix& matrixA = _matrixA;
	const BMatrix& matrixB = _matrixB;

	int skip = 0;
	BReal R1[4 * 3];
	BReal R2[4 * 3];
	for (int i = 0; i < 3; i++)
	{
		int row = 4 * i;
		BVector axisA = matrixA.column(i);
		R1[row + 0] = axisA.x();
		R1[row + 1] = axisA.y();
		R1[row + 2] = axisA.z();
		BVector axisB = matrixB.column(i);
		R2[row + 0] = axisB.x();
		R2[row + 1] = axisB.y();
		R2[row + 2] = axisB.z();
	}
	const BBox& boxA = cubeA->box();
	BVector dimensionA = boxA.veca() + boxA.vecb() + boxA.vecc();
	const BBox& boxB = cubeB->box();
	BVector dimensionB = boxB.veca() + boxB.vecb() + boxB.vecc();

	BVector extents1 = dimensionA + cubeA->margin();
	BVector extents2 = dimensionB + cubeB->margin();
	detecting(matrixA.position(), R1, extents1 * 2, matrixB.position(), R2, extents2 * 2);

	if (numContact > 0)
	{
		refreshContactPoints();
	}
}
void ICubeCubeAlgorithm::timeImpact()
{
	const BMatrix& matrixA = node_member(_bodyA)->matrix;
	const BMatrix& matrixB = node_member(_bodyB)->matrix;

	const BMatrix& interpMatrixA = body_member(_bodyA)->predictedMatrix;
	const BMatrix& interpMatrixB = body_member(_bodyB)->predictedMatrix;

	CastResult caseResult;
	IContinuousConvexCast ccc(_shapeA, _shapeB);
	ccc.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, caseResult);
}
